#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
Low-level servo protocol utilities.
"""

import logging
from typing import Dict

from ...config.settings import GimbalConfig

logger = logging.getLogger("RDK_YOLO")


class ServoProtocol:
    """
    生成舵机控制帧的协议封装。
    """

    FRAME_HEADER = bytes([0xAA, 0xBB])
    FRAME_TAIL = bytes([0xCC, 0xDD])

    def __init__(self, gimbal_config: GimbalConfig):
        self.config = gimbal_config
        self.supported_pins = gimbal_config.supported_pins or [gimbal_config.pan_pin, gimbal_config.tilt_pin]

    @staticmethod
    def _calculate_checksum(data: bytes) -> bytes:
        return bytes([sum(data) & 0xFF])

    @staticmethod
    def _angle_to_bytes(angle: float) -> bytes:
        angle_int = int(angle * 10)
        return bytes([angle_int >> 8, angle_int & 0xFF])

    def build_packet(self, servo_angles: Dict[int, float]) -> bytes:
        if len(servo_angles) > 9:
            raise ValueError("舵机数量超过协议限制(9)")

        data = bytearray([len(servo_angles)])

        for i in range(9):
            if i < len(self.supported_pins):
                pin = self.supported_pins[i]
                if pin in servo_angles:
                    data.append(pin)
                    data.extend(self._angle_to_bytes(servo_angles[pin]))
                else:
                    data.extend([0, 0, 0])
            else:
                data.extend([0, 0, 0])

        checksum = self._calculate_checksum(self.FRAME_HEADER + data)
        packet = self.FRAME_HEADER + data + checksum + self.FRAME_TAIL
        logger.debug("生成协议数据: %s", packet.hex())
        return packet

    @staticmethod
    def build_test_packet() -> bytes:
        return bytes.fromhex("AA BB 01 00 00 00 00 00 00 04 01 C2 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 2D CC DD")
